import numpy as np
import cv2

# 已知正方形的尺寸
square_size = 150 # mm
carb_xml = 'main_Para.xml'
# carb_xml = 'side_F_Para.xml'
# carb_xml = 'side_L_Para.xml'
fs = cv2.FileStorage(carb_xml, cv2.FILE_STORAGE_READ)
# 相机内参矩阵
# K = fs.getNode('camera-matrix').mat()
K = np.array([[3450, 0, 2100],
              [0, 3450, 1080],
              [0, 0, 1]])

def rotation_matrix_to_euler_angles(R):
    # 计算旋转向量
    rotation_vector, _ = cv2.Rodrigues(R)
    # 构造投影矩阵
    P = np.hstack((R, np.zeros((3, 1))))
    # 分解投影矩阵以获得欧拉角
    _, _, _, _, _, _, euler_angles = cv2.decomposeProjectionMatrix(P)
    return euler_angles

def homography_to_pose(H, K):
    # Compute the rotation and translation from the homography matrix H
    # and camera intrinsic matrix K.
    R_inv = np.linalg.inv(K) @ H
    s = np.linalg.norm(R_inv[:, :2], axis=0)
    scale = np.mean(s)
    if np.linalg.det(R_inv) < 0:
        scale = -scale
    R_inv = np.concatenate([R_inv[:, :2] / s, np.array([[0], [0], [1]])], axis=1)
    t = scale * np.linalg.inv(K) @ H[:, 2] / np.mean(s)
    R = np.linalg.inv(R_inv)
    return R, t

#像素分辨率
error = 0.125
# 正方形在像素坐标系下的四个角点坐标
# p1 = np.array([2100 - 900 - error, 1080 - 900 + error])
# p2 = np.array([2100 - 900 + error, 1080 + 900 - error])
# p3 = np.array([2100 + 900 - error, 1080 + 900 - error])
# p4 = np.array([2100 + 900 + error, 1080 - 900 + error])
# p1 = np.array([2100 - 900, 1080 - 900])
# p2 = np.array([2100 - 900, 1080 + 900])
# p3 = np.array([2100 + 900, 1080 + 900])
# p4 = np.array([2100 + 900, 1080 - 900])
p1 = np.array([1201.319, 181.319])
p2 = np.array([1198.681, 1978.681])
p4 = np.array([2998.681, 181.319])
p3 = np.array([3001.319, 1978.681])
image_points = np.array([p1, p2, p3, p4])

# 构建正方形的三维坐标矩阵
object_points = np.zeros((4, 3))
object_points[:, :2] = square_size * np.array([[-0.5, -0.5], [-0.5, 0.5], [0.5, 0.5], [0.5, -0.5]])

# 使用cv2.findHomography计算单应性变换矩阵
H, L = cv2.findHomography(image_points, object_points[:, :2])


# 通过H计算相机的位姿
retval, rotations, translations, normals = cv2.decomposeHomographyMat(H, K)
rot_vec, _ = cv2.Rodrigues(rotations[0])
theta = np.linalg.norm(rot_vec) / np.pi * 180
print(theta)
_, rvec, tvec = cv2.solvePnP(object_points, image_points, K, None)
Ra, _ = cv2.Rodrigues(rvec)
posea = np.column_stack((Ra, tvec))
arca = rotation_matrix_to_euler_angles(Ra)

R = rotations[0]
t = translations[0]
pose = np.column_stack((R, t))
arc = rotation_matrix_to_euler_angles(R)

print("相机的位姿：")
print(pose)
print(posea)
print(arc)
print(arca)
